#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fixmath.h>
#include <math.h>

//#define MODE_SPEED				1
//#define MODE_PERFORMANCE		2
//#define MODE_PRECISION			3
enum opt_t {
	MODE_SPEED = 1,
	MODE_PERFORMANCE,
	MODE_PRECISION	
};
typedef enum opt_t opt_t;


int main ( int argc, char* argv[] )
{
	float temp, a1, a2 = 2.0;
	int i = 0;
	int no_of_cycles;
	opt_t mode;
	fix16_t a1_16;
	fix16_t res_16;

	if( argc > 1 ) {
		if( strcmp( argv[1], "speed" ) == 0 ) {
			no_of_cycles = (int) atoi( (const char*) argv[2] );
			printf("Performance mode for: %d cycles\n", no_of_cycles);
			mode = MODE_SPEED;
		} 
		if( strcmp( argv[1], "perf" ) == 0 ) {
			no_of_cycles = (int) atoi( (const char*) argv[2] );
			printf("Performance mode for: %d cycles\n", no_of_cycles);
			mode = MODE_PERFORMANCE;
		} 
		if( strcmp( argv[1], "prec" ) == 0 ) {
			a1 = (float) atof( (const char*) argv[2] );
			printf("Precision mode for angle: %f\n", a1);
			mode = MODE_PRECISION;
		} 
	}

	switch( mode ) {
		case MODE_SPEED:
			a1 = (double) 0.6754;
			a1_16 = (fix16_t) fix16_from_dbl( (double) a1 );
			while( no_of_cycles-- )
			{
				res_16 = (fix16_t) fix16_asin((fix16_t) a1_16 );
			}
			printf("Fixed point math for asin(x) done for %s cycles\n", argv[2] );
		break;

		case MODE_PERFORMANCE:
			while( no_of_cycles-- )
			{
				a1 = (double) rand() / RAND_MAX;
				a1_16 = (fix16_t) fix16_from_dbl( (double) a1 );
				res_16 = (fix16_t) fix16_acos((fix16_t) a1_16 );
				res_16 = (fix16_t) fix16_atan((fix16_t) a1_16 );
			}
			printf("Fixed point math for asin(x) done for %s cycles\n", argv[2] );
		break;

		case MODE_PRECISION:
//			a1 = (double) rand() / RAND_MAX;
			printf("Angle = %f\n", a1 );
			a1_16 = (fix16_t) fix16_from_dbl( (double) a1 );
			printf("Converted into FIX16 = %f\n", fix16_to_dbl(a1_16));
			res_16 = (fix16_t) fix16_acos((fix16_t) a1_16 );
			printf("inverse cosine FIX16 = %f\n", fix16_to_dbl(res_16));
			res_16 = (fix16_t) fix16_atan((fix16_t) a1_16 );
			printf("inverse tangent FIX16 = %f\n", fix16_to_dbl(res_16));
			temp =  acos(a1);
			printf("inverse cosine = %f\n", temp );
			temp =  atan(a1);
			printf("inverse tangent = %f\n", temp );

			fix16_t a1_16 = fix16_from_dbl( (double) 0.7071 );
			fix16_t a2_16 = fix16_from_dbl( (double) 3.2979 );

			fix16_t res_16 = fix16_add( (fix16_t) a1_16, (fix16_t) a2_16 ); //4.005
			printf("adding: 4.005=%f\n", fix16_to_dbl((fix16_t) res_16 ));
			res_16 = fix16_sub( (fix16_t) a2_16, (fix16_t) a1_16 ); // 2.5908
			printf("subtract: 2.5908=%f\n", fix16_to_dbl((fix16_t) res_16 ));
			res_16 = fix16_mul( (fix16_t) a1_16, (fix16_t) a2_16 ); // 2.33194509
			printf("multiply: 2.33194509=%f\n", fix16_to_dbl((fix16_t) res_16 ));
			res_16 = (fix16_t) fix16_sqrt((fix16_t) a1_16 );
			printf("sqrt: 0.840892383=%f\n", fix16_to_dbl((fix16_t) res_16 ));
/*
			res_16 = (fix16_t) fix16_atan((fix16_t) a2_16 );
			printf("atan=%f\n", fix16_to_dbl((fix16_t) res_16 ));
			res_16 = (fix16_t) fix16_acos((fix16_t) a2_16 );
			printf("acos=%f\n", fix16_to_dbl((fix16_t) res_16 ));
			res_16 = (fix16_t) fix16_asin((fix16_t) a2_16 );
			printf("asin=%f\n", fix16_to_dbl((fix16_t) res_16 ));
*/

		break;

		default:
			printf("No mode selected.\n");
			printf("Usage: pftest <perf|speed> <count>\n\n");
			printf("perf - performance mode. It uses fixed point math to calculate asin.\n");
			printf("       resolution is set as 0.2 degree.\n");
			printf("prec - precision test. Compared with std. math.\n");

		break;
	}

	/*

	fix16_t a1_16 = fix16_from_dbl( (double) a1 );
	fix16_t a2_16 = fix16_from_dbl( (double) a2 );

	fix16_t res = fix16_add( (fix16_t) a1_16, (fix16_t) a2_16 ); //3.5
	printf("adding=%f\n", fix16_to_dbl((fix16_t) res ));
	res = fix16_sub( (fix16_t) a1_16, (fix16_t) a2_16 ); // 0.1
	printf("subtract=%f\n", fix16_to_dbl((fix16_t) res ));
	res = fix16_mul( (fix16_t) a1_16, (fix16_t) a2_16 ); // 7.26
	printf("multiply=%f\n", fix16_to_dbl((fix16_t) res ));
	res = (fix16_t) fix16_sqrt((fix16_t) a1_16 );
	printf("sqrt=%f\n", fix16_to_dbl((fix16_t) res ));
	res = (fix16_t) fix16_atan((fix16_t) a2_16 );
	printf("atan=%f\n", fix16_to_dbl((fix16_t) res ));
	res = (fix16_t) fix16_acos((fix16_t) a2_16 );
	printf("acos=%f\n", fix16_to_dbl((fix16_t) res ));
	res = (fix16_t) fix16_asin((fix16_t) a2_16 );
	printf("asin=%f\n", fix16_to_dbl((fix16_t) res ));

%	j = hypot(x,y)-coxa;
%
%	G=atan(y/x);
%	A0=atan(j/z);
%	A1=acos((femur^2+j^2+z^2-tibia^2)/(2*hypot(j,z)*femur));
%	B=acos((tibia^2+femur^2-z^2-j^2)/(2*femur*tibia));
	*/

	return 0;
}
